/*
 * @Author: liu Shihao
 * @Date: 2022-06-12 20:54:47
 * @LastEditors: liu Shihao
 * @LastEditTime: 2022-06-16 21:50:46
 * @FilePath: \bearpi-hm_nano\applications\BearPi\BearPi-HM_Nano\sample\robot\src\scs15_control.c
 * @Description: 空闲中断
 * Copyright (c) 2022 by ${fzu} email: logic_fzu@outlook.com, All Rights Reserved.
 */


#include "robot_gail.h"
#include "robot_main.h"
#include "robot_body.h"

int Abs(float a)
{
	return (a>=0)?a:-a;
}

/************线性规划************/
/*线性规划采用直线进行步态规划
**规划有Tort 步态
*/
void Linear_Trot_Move(struct Leg_t * leg)
{
	uint8_t cycle = 20;
	int StepLength = body.vx;         //X方向步长
	int LR_StepLength = body.vy;      //Y方向步长
	int R_StepLength =  body.rotate;             //旋转转向步长
	if(leg->state != leg->last_state) leg->count = 0;
	leg->count++;
	if(leg->state == Up)
	{
		leg->delta_x = -StepLength+leg->count*2*StepLength/cycle;
		leg->delta_y = -LR_StepLength+leg->count*2*LR_StepLength/cycle;
		leg->delta_xy = -R_StepLength+leg->count*2*R_StepLength/cycle;
	}
	else if(leg->state == Down)
	{
		leg->delta_x = StepLength-leg->count*2*StepLength/(cycle) ;
		leg->delta_y = LR_StepLength-leg->count*2*LR_StepLength/(cycle);
		leg->delta_xy = R_StepLength-leg->count*2*R_StepLength/cycle;
	}
	   //此处为全向移动（与旋转不同，旋转为对角变化）
	if(leg == &FR_Leg || leg == &BR_Leg)
	  {
		leg->delta_y = -leg->delta_y;   //Y方向向外为正  改变为同一方向
	  }
	     //旋转旋转变换
  if(leg == &FR_Leg || leg == &BL_Leg)
	{
		leg->delta_y =leg->delta_y -leg->delta_xy;
	}
	else 
	{
	    leg->delta_y =leg->delta_y +leg->delta_xy;
	}
}
/************线性规划************/
/*线性规划采用直线进行步态规划
**规划有Tort 步态
*/
void Linear_Trot_Gyrate(struct Leg_t * leg)
{
	uint8_t cycle = 30;
	int StepLength = body.vx;         //X方向步长
	int LR_StepLength = body.vy;      //Y方向步长
	if(leg->state != leg->last_state) leg->count = 0;
	leg->count++;
	if(leg->state == Up)
	{
		leg->delta_x = -StepLength+leg->count*2*StepLength/cycle;
		leg->delta_y = -LR_StepLength+leg->count*2*LR_StepLength/cycle;

	}
	else if(leg->state == Down)
	{
		leg->delta_x = StepLength-leg->count*2*StepLength/(cycle) ;
		leg->delta_y = LR_StepLength-leg->count*2*LR_StepLength/(cycle);
	}
	   //此处为全向移动（与旋转不同，旋转为对角变化）
	if(leg == &FR_Leg || leg == &BL_Leg)
	  {
		leg->delta_y = -leg->delta_y;   //Y方向向外为正  改变为同一方向
	  }
	     //旋转旋转变换
}


void Linear_Trot(void)
{
	float high=140;
	float cycle =30;                          
	static float count = 0;
	count ++;
	FR_Leg.last_state = FR_Leg.state;
	BL_Leg.last_state = BL_Leg.state;
	FL_Leg.last_state = FL_Leg.state;
	BR_Leg.last_state = BR_Leg.state;
	
	//tort
		if(count<cycle)
	{
		FR_Leg.state = Up;                          //右前腿
		BL_Leg.state = Up;
		FL_Leg.state = Down;
		BR_Leg.state = Down;
	   if(count <= cycle/2)
		{
			BL_Leg.delta_z=FR_Leg.delta_z = count*high/cycle;          //抬起，最高40mm
		}
		else
		{
			BL_Leg.delta_z=FR_Leg.delta_z = (cycle-count)*high/cycle;  //放下
		}
	}
	else if(count <= 2 * cycle)
	{
		FR_Leg.state = Down;
		BL_Leg.state = Down;                           //左后腿
		FL_Leg.state = Up;
		BR_Leg.state = Up;
		if(count <= cycle*3/2)
		{
			BR_Leg.delta_z=FL_Leg.delta_z = (count-cycle)*high/cycle;
		}
		else
		{
			BR_Leg.delta_z=FL_Leg.delta_z = (2*cycle-count)*high/cycle;
		}

	}
	else
	{
		count = 0;
	}	
}
void Linear_Trot_Wide(void)
{
	float high=60;
	float cycle =30;                          
	static float count = 0;
	static int roll_tim=0;
	count ++;
	FR_Leg.last_state = FR_Leg.state;
	BL_Leg.last_state = BL_Leg.state;
	FL_Leg.last_state = FL_Leg.state;
	BR_Leg.last_state = BR_Leg.state;
	if(count==cycle)
	{
 if(body.rotate==0)   {body.roll=0; printf("roll:%f \r\n",body.roll );}
     else if(body .rotate==30) 
			 { 
			if(roll_tim <30)    
					{
				 roll_tim++;
					 body.roll =89*roll_tim/cycle;
					}
		 else if(roll_tim ==30)
					{
				 body .roll =89;
				 roll_tim =30;
				}	
			 }
else if(body.rotate ==15)	
       {
		if(roll_tim >0)    
	      {
		   roll_tim--;
	       body.roll =89*roll_tim/cycle;
	      }
   else if(roll_tim ==0)
	      {
		   body .roll =0;
		   roll_tim =0;
		  }	   
       }
   }
float roll_delta_z = SHORT_LENGTH /4 *sin(body.roll /Rad_to_Degree);
	//tort
		if(count<cycle)
	{
		FR_Leg.state = Up;                          //右前腿
		BL_Leg.state = Up;
		FL_Leg.state = Down;
		BR_Leg.state = Down;
	   if(count <= cycle/2)
		{
			BL_Leg.delta_z = count*high/cycle - roll_delta_z+10;          //抬起，最高40mm
            FR_Leg.delta_z = count*high/cycle + roll_delta_z+10;
		}
		else
		{
			BL_Leg.delta_z = (cycle-count)*high/cycle - roll_delta_z+10;  //放下
		    FR_Leg.delta_z = (cycle-count)*high/cycle + roll_delta_z+10;
		}
	}
	else if(count <= 2 * cycle)
	{
		FR_Leg.state = Down;
		BL_Leg.state = Down;                           //左后腿
		FL_Leg.state = Up;
		BR_Leg.state = Up;
		if(count <= cycle*3/2)
		{   
			FL_Leg.delta_z = (count-cycle)*high/cycle - roll_delta_z+10;
			BR_Leg.delta_z = (count-cycle)*high/cycle + roll_delta_z+10;
		}
		else
		{
			FL_Leg.delta_z = (2*cycle-count)*high/cycle - roll_delta_z+10;
			BR_Leg.delta_z = (2*cycle-count)*high/cycle + roll_delta_z+10;
		}

	}
	else
	{
		count = 0;
	}	
}


